import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
import copy

def array2msg(pc):
    '''
    numpy array 转 ros::msgs
    :param pc:
    :return:
    '''
    points = pc[:,:]
    msg = PointCloud2()
    msg.header.stamp = rospy.Time().now()
    msg.header.frame_id = "base_link"

    # if len(points.shape) == 3:
    #     msg.height = points.shape[1]
    #     msg.width = points.shape[0]
    # else:
    msg.height = 1
    msg.width = len(points)

    msg.fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('', 12, PointField.FLOAT32, 1),
        PointField('normal_x', 16, PointField.FLOAT32, 1),
        PointField('normal_y', 20, PointField.FLOAT32, 1),
        PointField('normal_z', 24, PointField.FLOAT32, 1),
        PointField('', 28, PointField.FLOAT32, 1),
        PointField('intensity', 32, PointField.FLOAT32, 1),
        PointField('curvature', 36, PointField.FLOAT32, 1),
        PointField('', 40, PointField.FLOAT32, 1),
        PointField('', 44, PointField.FLOAT32, 1),
    ]

    msg.is_bigendian = False
    msg.point_step = 48
    msg.row_step = msg.point_step * points.shape[0]
    msg.is_dense = False
    msg.data = np.asarray(points, np.float32).tostring()
    # print("len: {}   {}".format(len(np.asarray(points, np.float32)),np.asarray(points, np.float32)[0,:]))
    # print("msg.row_step{} = msg.point_step{} * msg.width{} * msg.height{}".format(msg.row_step,msg.point_step,msg.width,msg.height))
    return msg

# def array2msg(pc):
#     '''
#     numpy array 转 ros::msgs
#     :param pc:
#     :return:
#     '''
#     points = pc[:,0:3]
#     msg = PointCloud2()
#     msg.header.stamp = rospy.Time().now()
#     msg.header.frame_id = "base_link"
#
#     if len(points.shape) == 3:
#         msg.height = points.shape[1]
#         msg.width = points.shape[0]
#     else:
#         msg.height = 1
#         msg.width = len(points)
#
#     msg.fields = [
#         PointField('x', 0, PointField.FLOAT32, 1),
#         PointField('y', 4, PointField.FLOAT32, 1),
#         PointField('z', 8, PointField.FLOAT32, 1)]
#     msg.is_bigendian = False
#     msg.point_step = 12
#     msg.row_step = msg.point_step * points.shape[0]
#     msg.is_dense = False
#     msg.data = np.asarray(points, np.float32).tostring()
#     return msg